from wlkata_mirobot_o import WlkataMirobot_o
from wlkata_mirobot import WlkataMirobot
from modbus_tk.defines import READ_COILS,READ_INPUT_REGISTERS,READ_HOLDING_REGISTERS
import modbus_tk.defines as cst  
import time, os, subprocess
import logging
import socket
import modbus_tk.modbus_tcp as modbus_tcp

import struct

robot_logger=logging.getLogger("Robot-Driver")

####示教点位
###六轴机器人
### 仓库物品起初放置位置
P1_cang_Put_up = [-62.54,8.7,-2,0,-6.69,62.54,99.7,-191.82,209.63,0,0,0,0]
P1_cang_Put = [-62.54,9.99,6.34,0,-16.34,62.54,99.72,-191.85,180.76,0,0,0,0]
P_transition = [-77.34,-33.03,31.06,0,1.25,32.09,30.52,-135.93,219.06,-0.38,-0.61,-45.25,0]

P2_cang_Put_up = [-74.39,13.48,-7.05,0,-6.42,74.39,60.54,-216.71,208.7,0,0,0,0]
P2_cang_Put = [-74.9,12.31,3.72,0,-16.03,74.91,57.49,-213.03,180.79,0,0,0,0]

P3_cang_Put_up = [-85.96,12.08,-4.28,0,-7.81,85.95,15.67,-221.88,205.2,0,0,0,0]
P3_cang_Put = [-85.96,13.26,2.67,0,-15.94,85.95,15.68,-221.89,180.7,0,0,0,0]

P4_cang_Put_up = [-97.88,7.66,1.67,0,-9.32,97.88,-29.35,-212.04,202.1,0,0,0,0]
P4_cang_Put = [-97.88,8.94,7.77,0,-16.7,97.88,-29.35,-212.04,180,0,0,0,0]

### 托盘 相对于机器人c从左至右：P1，P2---P3,P4
P1_plate_up = [-9.39,38.71,-40.72,0,2,-36.57,261.86,-43.29,212.9,0,0,-45.95,0]
P1_plate = [-10.27,38.38,-5.19,0,-33.19,-35.68,245.11,-44.4,111.61,0,0,-45.95,0]

P2_plate_up = [-18.88,32.46,-22.72,0,-9.74,-8.52,243.72,-83.37,184.96,0,0,-27.41,0]
P2_plate = [-17.43,40.98,-9.09,0,-31.89,-6,242.87,-76.25,111.99,0,0,-23.43,0]

P3_plate_up = [-13.29,-3.17,24.31,0,-21.15,6.52,183.49,-43.33,168.26,0,0,-6.76,0]
P3_plate = [-10.95,23.4,15.39,0,-38.81,8.2,212.88,-41.19,111.55,0,0,-2.75,0]

P4_plate_up = [-19.83,14.93,7.12,0.12,-22.23,34.36,208.58,-75.17,162.16,0,-0.21,14.48,0]
P4_plate = [-18.91,26.22,11.88,0.16,-38.26,36.45,210.8,-72.14,111.08,0,-0.2,17.45,0]

#### 皮带机上物品位置点--检测的话，就不需要了
P1_belt_Pick_Up = [25.52,16.02,-4.67,0.02,-11.19,-6.43,206.7,98.67,192.89,0,0.15,19.08,0]
P1_belt_Pick_ = [27.72,23.71,14.17,0.03,-37.74,-8.61,193.6,101.74,113.63,0,0.15,19.09,0]
P1_tmp = [172.6, -52.9, 211.4, 0, 0, 0]

P2_belt_Pick_Up = [18.99,6.01,12.56,0,-18.41,0.07,196.18,67.53,175.27,0,0.16,19.06,0]
P2_belt_Pick_ = [19.26,16.8,22.27,0,-38.92,-0.2,193.2,67.5,115.13,0,0.15,19.05,0]
P2_tmp = [172.6, -52.9, 211.4, 0, 0, 0]

P3_belt_Pick_Up = [29.99,1.04,19.98,0.03,-20.86,-11,170.18,98.22,168.74,0,0.16,18.98,0]
P3_belt_Pick_ = [32.26,10.25,30.62,0.05,-40.7,-13.2,160.41,101.26,113.54,0,0.17,19.02,0]
P3_tmp = [172.6, -52.9, 211.4, 0, 0, 0]

P4_belt_Pick_Up = [22.94,-8.22,32.02,0.02,-23.62,-3.98,162.88,68.94,159.72,0,0.18,18.95,0]
P4_belt_Pick_ = [24.06,-0.37,41.56,0.02,-41.02,-5.07,154.54,69.01,114.49,0,0.17,18.98,0]
P4_tmp = [172.6, -52.9, 211.4, 0, 0, 0]


### 仓库物品最终放置位置 red:1  green:2  blue:3  yellow:4号仓库
P_put_Red_Up = [-63.79,-14.49,31.56,-0.09,-17.8,33.43,75.21,-152.88,176.8,-0.47,-0.55,-30.33,0]
P_put_Red = [-63.79,-5.41,39.94,-0.11,-35.23,33.41,75.2,-152.86,130.93,-0.46,-0.53,-30.32,0]

P_put_green_Up = [-75.29,-7.46,24.03,-0.27,-17.3,18.05,46.51,-177.66,180.77,-0.47,-0.61,-57.17,0]
P_put_green = [-77.85,-1.65,35.03,-0.25,-34.11,20.57,37.59,-175.01,134.4,-0.45,-0.61,-57.14,0]

P_put_blue_Up = [-90.41,-7.05,26.75,-0.98,-17.56,12.25,-1.7,-181.35,171.78,-0.45,2.3,-77.86,0]
P_put_blue = [-90.45,-2.14,35.59,-0.94,-31.26,9.09,-1.74,-176.73,134.21,-0.43,2.29,-80.87,0]

P_put_yellow_Up = [-104.82,-20.41,38.81,-0.56,-19.03,-5.43,-40.88,-153.6,169.58,-0.47,-0.68,-110.07,0]
P_put_yellow = [-103.68,-12.65,44.75,-1.84,-28.93,-7.3,-38.13,-153.82,135.3,-1.96,2.95,-110.08,0]

####四轴机器人
P_Pick_Up = [56.2, 142.7, 130.8, 96.5] 
P_Pick_ = [60.2, 138.7, 52.8, 92.5]
P_get = [61.2, 201.7, 48.8, 92.5]
P_tmp = [68.3, 198.2, 132.0, 92.5]
P_Put_up = [188.9, -143.5, 127.8, 96.4]
# P_Put = [192.9, -141.5, 51.8, 96.4]
P_Put = [192.9, -215.5, 51.8, 96.4]
P_Out = [192.9, -215.5, 51.8, 96.4]
P_Height = [192.9, -215.5, 89.8, 96.4]

arm = WlkataMirobot()
arm_ = WlkataMirobot_o(portname='/dev/ttyUSB0') 

current_file_path = os.path.abspath(__file__)
current_dir_path = os.path.dirname(current_file_path)
pwd_path = os.path.join(current_dir_path, '')


class Robot_Driver():  
    arm.home()
    time.sleep(1)
    arm_.home()
    
    Emer_flag =0
    
    def get_Robot_Status(self, ):
        try:    
            regValue = arm.status.state
            print(f"机器人状态{regValue}")
            # regValue = arm.get_status.state
            return regValue
        except Exception as e:
            robot_logger.error("can not get robot {}'s status! Reason:{}".format(e))
            return None
        
    def get_KDI(self):
        pass

    def robot_Home(self):
        arm.home()
        arm_.home()

    def robot_Stop(self):
        arm.stop()
        arm_.stop_6axis()

    def control_IO(self):
        arm.Master.set_DO(2,1)
        # arm.Master.set_DO(1,1)
        time.sleep(4)
        arm.Master.set_DO(2,0)
        # arm.Master.set_DO(1,0)

    def robot_getGoods(self):
        action_name = "getGoods.py"
        file_path = pwd_path + action_name
        self.robot_do  = subprocess.Popen(["python3",file_path])   
        time.sleep(2)
        while self.robot_do.poll() is None:
            time.sleep(0.5)


    def robot_getQR(self, num):
        match num:
            case 1:
                ### 抓取点上方
                arm.set_joint_angle(P = P1_belt_Pick_Up)
                #### 抓取点
                arm.set_joint_angle(P = P1_belt_Pick_)
                #### 吸气
                arm.Master.set_DO(0,1)
                time.sleep(1)
                arm.set_joint_angle(P = P1_belt_Pick_Up)
            case 2:
                ### 抓取点上方
                arm.set_joint_angle(P = P2_belt_Pick_Up)
                #### 抓取点
                arm.set_joint_angle(P = P2_belt_Pick_) 
                #### 吸气
                arm.Master.set_DO(0,1)
                arm.set_joint_angle(P = P2_belt_Pick_Up)
                time.sleep(1)
            case 3:
                #### 抓取点上方
                arm.set_joint_angle(P = P3_belt_Pick_Up)
                #### 抓取点
                arm.set_joint_angle(P = P3_belt_Pick_) 
                #### 吸气
                arm.Master.set_DO(0,1)
                time.sleep(1)

            case 4:
                #### 抓取点上方
                arm.set_joint_angle(P = P4_belt_Pick_Up)
                #### 抓取点
                arm.set_joint_angle(P = P4_belt_Pick_) 
                #### 吸气
                arm.Master.set_DO(0,1)
                time.sleep(1)

    def robot_PutQR(self,num):
        match num:
            case 1:
                #### 过渡点
                arm.set_joint_angle(P = P_transition)
                # arm.set_joint_angle(P = P1_tmp)
                ### 放置点上方
                arm.set_joint_angle(P = P_put_Red_Up) 
                #### 放置点
                arm.set_joint_angle(P = P_put_Red)                   
                arm.Master.set_DO(0,0)
                time.sleep(1)
                arm.set_joint_angle(P = P_put_Red_Up) 
                #### 回零点
                arm.set_tool_pose(x=198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)
                
            case 2:
                #### 过渡点
                arm.set_joint_angle(P = P_transition)
                ### 放置点上方
                arm.set_joint_angle(P = P_put_green_Up) 
                #### 放置点
                arm.set_joint_angle(P = P_put_green)                   
                arm.Master.set_DO(0,0)
                time.sleep(1)
                arm.set_joint_angle(P = P_put_green_Up) 
                #### 回零点
                arm.set_tool_pose(x =198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)
                
            case 3:
                arm.set_joint_angle(P = P_transition)
                ### 放置点上方
                arm.set_joint_angle(P = P_put_blue_Up) 
                #### 放置点
                arm.set_joint_angle(P = P_put_blue)                   
                arm.Master.set_DO(0,0)
                time.sleep(1)
                arm.set_joint_angle(P = P_put_blue_Up)
                #### 回零点
                arm.set_tool_pose(x=198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)   
                
            case 4:
                arm.set_joint_angle(P = P4_belt_Pick_Up)
                #### 过渡点
                arm.set_joint_angle(P = P_transition)
                ### 放置点上方
                arm.set_joint_angle(P = P_put_yellow_Up) 
                #### 放置点
                arm.set_joint_angle(P = P_put_yellow)                   
                arm.Master.set_DO(0,0)
               
                time.sleep(1)
                arm.set_joint_angle(P = P_put_yellow_Up) 
                #### 回零点
                arm.set_tool_pose(x=198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)     

    #### 入库（下方仓位）
    def robot_Action(self,color_num):
        match color_num:
            ###"red"
            case 1:
                ### 抓取点上方
                arm.set_joint_angle(P = P1_belt_Pick_Up)
                #### 抓取点
                arm.set_joint_angle(P = P1_belt_Pick_) 
                #### 吸气
                arm.Master.set_DO(0,1)
                time.sleep(1)
                arm.set_joint_angle(P = P1_belt_Pick_Up)
                #### 过渡点
                arm.set_joint_angle(P = P_transition)
                ### 放置点上方
                arm.set_joint_angle(P = P_put_Red_Up) 
                #### 放置点
                arm.set_joint_angle(P = P_put_Red)                   
                arm.Master.set_DO(0,0)
                time.sleep(1)
                arm.set_joint_angle(P = P_put_Red_Up) 
                #### 回零点
                arm.set_joint_angle(x=198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)
            
            ###"green"
            case 2:
                ### 抓取点上方
                arm.set_joint_angle(P = P2_belt_Pick_Up)
                #### 抓取点
                arm.set_joint_angle(P = P2_belt_Pick_) 
                #### 吸气
                arm.Master.set_DO(0,1)
                arm.set_joint_angle(P = P2_belt_Pick_Up)
                time.sleep(1)
                #### 过渡点
                arm.set_joint_angle(P = P_transition)
                ### 放置点上方
                arm.set_joint_angle(P = P_put_green_Up) 
                #### 放置点
                arm.set_joint_angle(P = P_put_green)                   
                arm.Master.set_DO(0,0)
                time.sleep(1)
                arm.set_joint_angle(P = P_put_green_Up) 
                #### 回零点
                arm.set_tool_pose(x =198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)
            
            ### "blue"
            case 3:
                #### 抓取点上方
                arm.set_joint_angle(P = P3_belt_Pick_Up)
                #### 抓取点
                arm.set_joint_angle(P = P3_belt_Pick_) 
                #### 吸气
                arm.Master.set_DO(0,1)
                time.sleep(1)
                arm.set_joint_angle(P = P3_belt_Pick_Up)
                #### 过渡点
                arm.set_joint_angle(P = P_transition)
                ### 放置点上方
                arm.set_joint_angle(P = P_put_blue_Up) 
                #### 放置点
                arm.set_joint_angle(P = P_put_blue)                   
                arm.Master.set_DO(0,0)
                time.sleep(1)
                arm.set_joint_angle(P = P_put_blue_Up)
                #### 回零点
                arm.set_tool_pose(x=198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)
            
            ### "yellow"
            case 4:
                #### 抓取点上方
                arm.set_joint_angle(P = P4_belt_Pick_Up)
                #### 抓取点
                arm.set_joint_angle(P = P4_belt_Pick_) 
                #### 吸气
                arm.Master.set_DO(0,1)
                time.sleep(1)
                arm.set_joint_angle(P = P4_belt_Pick_Up)
                #### 过渡点
                arm.set_joint_angle(P = P_transition)
                ### 放置点上方
                arm.set_joint_angle(P = P_put_yellow_Up) 
                #### 放置点
                arm.set_joint_angle(P = P_put_yellow)                   
                arm.Master.set_DO(0,0)
                time.sleep(1)
                arm.set_joint_angle(P = P_put_yellow_Up) 
                #### 回零点
                arm.set_tool_pose(x=198.67,y=0.0,z=230.72,roll=0.0,pitch=0.0,yaw=0.0)

    def robot_putPlates2Belt(self):
        ### 抓取点上方
        arm_.set_tool_pose(x = P_Pick_Up[0], y = P_Pick_Up[1], z = P_Pick_Up[2],  roll =P_Pick_Up[3])
        #### 抓取点
        arm_.set_tool_pose(x = P_Pick_[0], y = P_Pick_[1], z = P_Pick_[2],  roll =P_Pick_[3]) 
        arm_.set_tool_pose(x = P_get[0], y = P_get[1], z = P_get[2],  roll =P_get[3])
        time.sleep(1)
        #### 过渡点
        arm_.set_tool_pose(x = P_tmp[0], y = P_tmp[1], z = P_tmp[2],  roll =P_tmp[3])
        ### 放置点上方
        arm_.set_tool_pose(x = P_Put_up[0], y = P_Put_up[1], z = P_Put_up[2],  roll =P_Put_up[3]) 
        #### 放置点
        arm_.set_tool_pose(x = P_Put[0], y = P_Put[1], z = P_Put[2],  roll =P_Put[3])                   
        arm_.set_tool_pose(x = P_Out[0], y = P_Out[1], z = P_Out[2],  roll =P_Out[3])
        time.sleep(1)
        #### 回零点
        arm_.set_tool_pose(x = P_Height[0], y = P_Height[1], z = P_Height[2],  roll =P_Height[3])

    def robot_reset(self):
        arm.home()
